
//抖音：暴改车间主任
//微信公众号：爆改车间
//新迷你四足机器人代码
//测试环境： 开发板：合宙ESP32C3 IDE版本：1.8.19  ESP32 SDK：2.0.5
//项目详细介绍：立创开源平台搜索：迷你四足机器人

#include <TFT_eSPI.h> // Graphics and font library for ILI9341 driver chip
#include <SPI.h>
TFT_eSPI tft = TFT_eSPI();  // Invoke library
TFT_eSprite img = TFT_eSprite(&tft);
#define DEG2RAD 0.0174532925

//蓝牙部分

#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>

BLEServer *pServer = NULL;
BLECharacteristic * pTxCharacteristic;
bool deviceConnected = false;
bool oldDeviceConnected = false;
uint8_t txValue = 0;


//String chipId;
// See the following for generating UUIDs:
// https://www.uuidgenerator.net/

#define SERVICE_UUID           "ce409cbe-8cf5-4de5-ac1c-d1123b4e2544" // UART service UUID
#define CHARACTERISTIC_UUID_RX "ce409cb1-8cf5-4de5-ac1c-d1123b4e2544"
#define CHARACTERISTIC_UUID_TX "ce409cb2-8cf5-4de5-ac1c-d1123b4e2544"


class MyServerCallbacks: public BLEServerCallbacks {
    void onConnect(BLEServer* pServer) {
      deviceConnected = true;
    };

    void onDisconnect(BLEServer* pServer) {
      deviceConnected = false;
    }
};
String resStr;
String chipId;
String line;
uint8_t cmd = 0;

class MyCallbacks: public BLECharacteristicCallbacks {
    void onWrite(BLECharacteristic *pCharacteristic) {
      std::string rxValue = pCharacteristic->getValue();

      if (rxValue.length() > 0) {
        Serial.println("*********");
        Serial.print("Received Value: ");
        for (int i = 0; i < rxValue.length(); i++){
          Serial.print(rxValue[i]);
          resStr += rxValue[i];
        }
        Serial.println();
        Serial.println("*********");
        line = resStr;
        if(line.indexOf("CMD") != -1){ 
             String tline = line.substring(3);
    
             Serial.println(tline);
             cmd = tline.toInt();
    
    
      }
        resStr = "";
      }
    }
};


//************

#define TIMER_INTERRUPT_DEBUG       0
#define ISR_SERVO_DEBUG             0

// Select different ESP32 timer number (0-1) to avoid conflict
#define USE_ESP32_TIMER_NO          1

#include "ESP32_C3_ISR_Servo.h"   //C3使用中断方式进行舵机驱动库

//  -----               -----
// | D5  |             | D1 |
// |IO18 |             |IO0 |
//  ----- -----   ----- -----
//       | D6  | | D2  |
//       |IO19 | |IO1  |
//        -----   -----
//       | D7  | | D3  |
//       | IO8 | |IO12 |
//  ----- -----   ----- -----
// | D8  |             | D4  |
// |IO9  |             |IO13 |
//  -----               -----
//
#define PIN_D0            0         
#define PIN_D1            1         
#define PIN_D2            12       
#define PIN_D3            13         
#define PIN_D4            18         
#define PIN_D5            19         
#define PIN_D6            8         
#define PIN_D7            9        

// Published values for SG90 servos; adjust if needed
#define MIN_MICROS      800  //544
#define MAX_MICROS      2450



uint16_t lx,ly,rx,ry;
uint8_t sit_mode;


const int numberOfServos = 8; // Number of servos
const int numberOfACE = 9; // Number of action code elements
int servoCal[] = { 10, 5, 0, -13, -10, 10, -3, 5 }; // Servo calibration data 舵机偏移量校准
int servoPos[] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // Servo current position
int servoPrevPrg[] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // Servo previous prg
int servoPrgPeriod = 20; // 50 ms
int servoIndex[numberOfServos]; // Servo object

// Servo zero position
int servoAct00 [] PROGMEM =
  // P0 , P1 , P12, P13, P18, P19, P8 , P9
{  135,  45, 135,  45,  45, 135,  45, 135 };

// Zero
int servoPrg00step = 1;
int servoPrg00 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {  135,  45, 135,  45,  45, 135,  45, 135, 1000  }, // zero position
};

// Standby
int servoPrg01step = 2;
int servoPrg01 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   90,  90,  90,  90,  90,  90,  90,  90,  200  }, // prep standby
  {  -20,   0,   0,  20,  20,   0,   0, -20,  200  }, // standby
};

// Forward
int servoPrg02step = 11;
int servoPrg02 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {   20,   0,   0,   0,   0,   0, -45,  20,  100  }, // leg1,4 up; leg4 fw
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg1,4 dn
  {    0,   0,   0, -20, -20,   0,   0,   0,  100  }, // leg2,3 up
  {    0, -45,  45,   0,   0,   0,  45,   0,  100  }, // leg1,4 bk; leg2 fw
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg2,3 dn
  {   20,  45,   0,   0,   0,   0,   0,  20,  100  }, // leg1,4 up; leg1 fw
  {    0,   0, -45,   0,   0,  45,   0,   0,  100  }, // leg2,3 bk
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg1,4 dn
  {    0,   0,   0,   0, -20,   0,   0,   0,  100  }, // leg3 up
  {    0,   0,   0,   0,  20, -45,   0,   0,  100  }, // leg3 fw dn
};

// Backward
int servoPrg03step = 11;
int servoPrg03 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {   20, -45,   0,   0,   0,   0,   0,  20,  100  }, // leg4,1 up; leg1 fw
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg4,1 dn
  {    0,   0,   0, -20, -20,   0,   0,   0,  100  }, // leg3,2 up
  {    0,  45,   0,   0,   0,  45, -45,   0,  100  }, // leg4,1 bk; leg3 fw
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg3,2 dn
  {   20,   0,   0,   0,   0,   0,  45,  20,  100  }, // leg4,1 up; leg4 fw
  {    0,   0,  45,   0,   0, -45,   0,   0,  100  }, // leg3,1 bk
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg4,1 dn
  {    0,   0,   0, -20,   0,   0,   0,   0,  100  }, // leg2 up
  {    0,   0, -45,  20,   0,   0,   0,   0,  100  }, // leg2 fw dn
};

// Move Left
int servoPrg04step = 11;
int servoPrg04 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {    0,   0, -45, -20, -20,   0,   0,   0,  100  }, // leg3,2 up; leg2 fw
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg3,2 dn
  {   20,   0,   0,   0,   0,   0,   0,  20,  100  }, // leg1,4 up
  {    0,  45,  45,   0,   0, -45,   0,   0,  100  }, // leg3,2 bk; leg1 fw
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg1,4 dn
  {    0,   0,   0, -20, -20,  45,   0,   0,  100  }, // leg3,2 up; leg3 fw
  {    0, -45,   0,   0,   0,   0,  45,   0,  100  }, // leg1,4 bk
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg3,2 dn
  {    0,   0,   0,   0,   0,   0,   0,  20,  100  }, // leg4 up
  {    0,   0,   0,   0,   0,   0, -45, -20,  100  }, // leg4 fw dn
};

// Move Right
int servoPrg05step = 11;
int servoPrg05 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {    0,   0,   0, -20, -20, -45,   0,   0,  100  }, // leg2,3 up; leg3 fw
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg2,3 dn
  {   20,   0,   0,   0,   0,   0,   0,  20,  100  }, // leg4,1 up
  {    0,   0, -45,   0,   0,  45,  45,   0,  100  }, // leg2,3 bk; leg4 fw
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg4,1 dn
  {    0,   0,  45, -20, -20,   0,   0,   0,  100  }, // leg2,3 up; leg2 fw
  {    0,  45,   0,   0,   0,   0, -45,   0,  100  }, // leg4,1 bk
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg2,3 dn
  {   20,   0,   0,   0,   0,   0,   0,   0,  100  }, // leg1 up
  {  -20, -45,   0,   0,   0,   0,   0,   0,  100  }, // leg1 fw dn
};

// Turn left
int servoPrg06step = 8;
int servoPrg06 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {   20,   0,   0,   0,   0,   0,   0,  20,  100  }, // leg1,4 up
  {    0,  45,   0,   0,   0,   0,  45,   0,  100  }, // leg1,4 turn
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg1,4 dn
  {    0,   0,   0, -20, -20,   0,   0,   0,  100  }, // leg2,3 up
  {    0,   0,  45,   0,   0,  45,   0,   0,  100  }, // leg2,3 turn
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg2,3 dn
  {    0, -45, -45,   0,   0, -45, -45,   0,  100  }, // leg1,2,3,4 turn
};

// Turn right
int servoPrg07step = 8;
int servoPrg07 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  100  }, // standby
  {    0,   0,   0, -20, -20,   0,   0,   0,  100  }, // leg2,3 up
  {    0,   0, -45,   0,   0, -45,   0,   0,  100  }, // leg2,3 turn
  {    0,   0,   0,  20,  20,   0,   0,   0,  100  }, // leg2,3 dn
  {   20,   0,   0,   0,   0,   0,   0,  20,  100  }, // leg1,4 up
  {    0, -45,   0,   0,   0,   0, -45,   0,  100  }, // leg1,4 turn
  {  -20,   0,   0,   0,   0,   0,   0, -20,  100  }, // leg1,4 dn
  {    0,  45,  45,   0,   0,  45,  45,   0,  100  }, // leg1,2,3,4 turn
};

// Lie
int servoPrg08step = 1;
int servoPrg08 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {  110,  90,  90,  70,  70,  90,  90, 110,  500  }, // leg1,4 up
};

// Say Hi
int servoPrg09step = 4;
int servoPrg09 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {  120,  90,  90, 110,  60,  90,  90,  70,  200  }, // leg1, 3 down
  {  -50,   0,   0,   0,  50,   0,   0,   0,  200  }, // standby
  {   50,   0,   0,   0, -50,   0,   0,   0,  200  }, // leg1, 3 down
  {  -50,   0,   0,   0,  50,   0,   0,   0,  200  }, // standby
};

// Fighting
int servoPrg10step = 11;
int servoPrg10 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {  120,  90,  90, 110,  60,  90,  90,  70,  200  }, // leg1, 2 down
  {    0, -20, -20,   0,   0, -20, -20,   0,  200  }, // body turn left
  {    0,  40,  40,   0,   0,  40,  40,   0,  200  }, // body turn right
  {    0, -40, -40,   0,   0, -40, -40,   0,  200  }, // body turn left
  {    0,  40,  40,   0,   0,  40,  40,   0,  200  }, // body turn right
  {  -50, -20, -20, -40,  50, -20, -20,  40,  200  }, // leg1, 2 up ; leg3, 4 down
  {    0, -20, -20,   0,   0, -20, -20,   0,  200  }, // body turn left
  {    0,  40,  40,   0,   0,  40,  40,   0,  200  }, // body turn right
  {    0, -40, -40,   0,   0, -40, -40,   0,  200  }, // body turn left
  {    0,  40,  40,   0,   0,  40,  40,   0,  200  }, // body turn right
  {    0, -20, -20,   0,   0, -20, -20,   0,  200  }, // leg1, 2 up ; leg3, 4 down
};

// Push up
int servoPrg11step = 11;
int servoPrg11 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  300  }, // start
  {   30,   0,   0, -30, -30,   0,   0,  30,  400  }, // down
  {  -30,   0,   0,  30,  30,   0,   0, -30,  500  }, // up
  {   30,   0,   0, -30, -30,   0,   0,  30,  600  }, // down
  {  -30,   0,   0,  30,  30,   0,   0, -30,  700  }, // up
  {   30,   0,   0, -30, -30,   0,   0,  30,  1300 }, // down
  {  -30,   0,   0,  30,  30,   0,   0, -30,  1800 }, // up
  {   65,   0,   0, -65, -65,   0,   0,  65,  200  }, // fast down
  {  -65,   0,   0,   0,  15,   0,   0,   0,  500  }, // leg1 up
  {    0,   0,   0,   0,  50,   0,   0,   0,  500  }, // leg2 up
  {    0,   0,   0,  65,   0,   0,   0, -65,  500  }, // leg3, leg4 up
};

// Sleep
int servoPrg12step = 3;
int servoPrg12 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   35,  90,  90, 145, 145,  90,  90,  35,  400  }, // leg1,4 dn
  {    0, -45,  45,   0,   0,  45, -45,   0,  400  }, // protect myself
  {   80,   0,  0, -90,-90,   0,   0, +80,  1000  }, // protect myself
};

// Dancing 1
int servoPrg13step = 10;
int servoPrg13 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   90,  90,  90,  90,  90,  90,  90,  90,  300  }, // leg1,2,3,4 up
  {  -40,   0,   0,   0,   0,   0,   0,   0,  300  }, // leg1 dn
  {   40,   0,   0,  40,   0,   0,   0,   0,  300  }, // leg1 up; leg2 dn
  {    0,   0,   0, -40,   0,   0,   0, -40,  300  }, // leg2 up; leg4 dn
  {    0,   0,   0,   0,  40,   0,   0,  40,  300  }, // leg4 up; leg3 dn
  {  -40,   0,   0,   0, -40,   0,   0,   0,  300  }, // leg3 up; leg1 dn
  {   40,   0,   0,  40,   0,   0,   0,   0,  300  }, // leg1 up; leg2 dn
  {    0,   0,   0, -40,   0,   0,   0, -40,  300  }, // leg2 up; leg4 dn
  {    0,   0,   0,   0,  40,   0,   0,  40,  300  }, // leg4 up; leg3 dn
  {    0,   0,   0,   0, -40,   0,   0,   0,  300  }, // leg3 up
};

// Dancing 2
int servoPrg14step = 9;
int servoPrg14 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  45, 135, 110, 110, 135,  45,  70,  300  }, // leg1,2,3,4 two sides
  {   45,   0,   0, -45,   0,   0,   0,   0,  300  }, // leg1,2 up
  {  -45,   0,   0,  45, -45,   0,   0,  45,  300  }, // leg1,2 dn; leg3,4 up
  {   45,   0,   0, -45,  45,   0,   0, -45,  300  }, // leg3,4 dn; leg1,2 up
  {  -45,   0,   0,  45, -45,   0,   0,  45,  300  }, // leg1,2 dn; leg3,4 up
  {   45,   0,   0, -45,  45,   0,   0, -45,  300  }, // leg3,4 dn; leg1,2 up
  {  -45,   0,   0,  45, -45,   0,   0,  45,  300  }, // leg1,2 dn; leg3,4 up
  {   45,   0,   0, -45,  45,   0,   0, -45,  300  }, // leg3,4 dn; leg1,2 up
  {  -40,   0,   0,  40,   0,   0,   0,   0,  300  }, // leg1,2 dn
};

// Dancing 3
int servoPrg15step = 10;
int servoPrg15 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  45,  45, 110, 110, 135, 135,  70,  300  }, // leg1,2,3,4 bk
  {   40,   0,   0, -50, -40,   0,   0,   0,  300  }, // leg1,2,3 up
  {  -40,   0,   0,  50,  40,   0,   0,   0,  300  }, // leg1,2,3 dn
  {   40,   0,   0,   0, -40,   0,   0,  50,  300  }, // leg1,3,4 up
  {  -40,   0,   0,   0,  40,   0,   0, -50,  300  }, // leg1,3,4 dn
  {   40,   0,   0, -50, -40,   0,   0,   0,  300  }, // leg1,2,3 up
  {  -40,   0,   0,  50,  40,   0,   0,   0,  300  }, // leg1,2,3 dn
  {   40,   0,   0,   0, -40,   0,   0,  50,  300  }, // leg1,3,4 up
  {  -40,   0,   0,   0,  40,   0,   0, -50,  300  }, // leg1,3,4 dn
  {    0,  45,  45,   0,   0, -45, -45,   0,  300  }, // standby
};

// up
int servoPrg16step = 1;
int servoPrg16 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   60,  90,  90, 120, 120,  90,  90,  60,  100  }, // standby
};
//Get down
int servoPrg17step = 1;
int servoPrg17 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   110,  90,  90,   60,   60,  90,  90,  110,  300 },
//  {   +20,   0,   0,  -20,  -20,   0,   0,  +20,  300 },
//  {     0,   0,   0,    0,    0,   0,   0,    0,  1000 },
};

//holle
int servoPrg18step = 9;
int servoPrg18 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   0,  0,     0,   0,   0,  -60,  0,  0,  200  },
  {   +150,  +50,  0, 0, 0,  0,  0,  0,  300  },
  {   -70,  0,  0, 0, 0,  0,  0,  0,  300  },
  {   +70,  0,  0, 0, 0,  0,  0,  0,  300  },
  {   -70,  0,  0, 0, 0,  0,  0,  0,  300  },
  {   +70,  0,  0, 0, 0,  0,  0,  0,  300  },
  {   -70,  0,  0, 0, 0,  0,  0,  0,  300  },
  {   +70,  0,  0, 0, 0,  0,  0,  0,  300  },
//  {   +20,   0,   0,  -20,  -20,   0,   0,  +20,  300 },
//  {     0,   0,   0,    0,    0,   0,   0,    0,  1000 },
};

//Stand up
int servoPrg19step = 1;
int servoPrg19 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   0,  90,  90, 180, 180,  90,  90,  0,  100 },
};

//

int servoPrg20step = 9;
int servoPrg20 [][numberOfACE] PROGMEM = {
  // P0 , P1 , P12, P13, P18, P19, P8 , P9,  ms
  {   70,  90,  90, 110, 110,  90,  90,  70,  200  }, // standby
  {   0,  +60,  0,    0,   0, -60,  0,  0,  200  },
  {   +150, 0,  0,    0,-150,  0,   0,  0,  300  },
  {   -70,  0,  0,    0,   0,  0,   0,  0,  300  },
  {   +70,  0,  0,    0, +70,  0,   0,  0,  300  },
  {   -70,  0,  0,    0, -70,  0,   0,  0,  300  },
  {   +70,  0,  0,    0, +70,  0,   0,  0,  300  },
  {   -70,  0,  0,    0, -70,  0,   0,  0,  300  },
  {   +70,  0,  0,    0, +70,  0,   0,  0,  300  },
}; 

void setup() {
  Serial.begin(115200);
  Serial.println("Robot Starts Initialization");

  tft.init();
  tft.setRotation(0);
  tft.fillScreen(TFT_BLACK);
  
//---------蓝牙连接初始化

  chipId = String((uint32_t)ESP.getEfuseMac(), HEX);
  chipId.toUpperCase();
//  chipid =ESP.getEfuseMac();
//  Serial.printf("Chip id: %s\n", chipid.c_str());
  Serial.println(chipId);
  // Create the BLE Device
  BLEDevice::init("BGxiaosizu");

  // Create the BLE Server
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  // Create the BLE Service
  BLEService *pService = pServer->createService(SERVICE_UUID);

  // Create a BLE Characteristic
  pTxCharacteristic = pService->createCharacteristic(
                        CHARACTERISTIC_UUID_TX,
                        BLECharacteristic::PROPERTY_NOTIFY
                      );

  pTxCharacteristic->addDescriptor(new BLE2902());

  BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(
      CHARACTERISTIC_UUID_RX,
      BLECharacteristic::PROPERTY_WRITE
                                          );

  pRxCharacteristic->setCallbacks(new MyCallbacks());

  // Start the service
  pService->start();

  // Start advertising
  pServer->getAdvertising()->start();
  Serial.println("Waiting a client connection to notify...");

//------------初始化舵机控管脚

  ESP32_ISR_Servos.useTimer(USE_ESP32_TIMER_NO);

  servoIndex[0] = ESP32_ISR_Servos.setupServo(PIN_D0, MIN_MICROS, MAX_MICROS);
  servoIndex[1] = ESP32_ISR_Servos.setupServo(PIN_D1, MIN_MICROS, MAX_MICROS);
  servoIndex[2] = ESP32_ISR_Servos.setupServo(PIN_D2, MIN_MICROS, MAX_MICROS);
  servoIndex[3] = ESP32_ISR_Servos.setupServo(PIN_D3, MIN_MICROS, MAX_MICROS);
  servoIndex[4] = ESP32_ISR_Servos.setupServo(PIN_D4, MIN_MICROS, MAX_MICROS);
  servoIndex[5] = ESP32_ISR_Servos.setupServo(PIN_D5, MIN_MICROS, MAX_MICROS);
  servoIndex[6] = ESP32_ISR_Servos.setupServo(PIN_D6, MIN_MICROS, MAX_MICROS);
  servoIndex[7] = ESP32_ISR_Servos.setupServo(PIN_D7, MIN_MICROS, MAX_MICROS);

  ESP32_ISR_Servos.setPosition(servoIndex[0], 90 + servoCal[0]);
  ESP32_ISR_Servos.setPosition(servoIndex[1], 90 + servoCal[1]);
  ESP32_ISR_Servos.setPosition(servoIndex[2], 90 + servoCal[2]);
  ESP32_ISR_Servos.setPosition(servoIndex[3], 90 + servoCal[3]);
  ESP32_ISR_Servos.setPosition(servoIndex[4], 90 + servoCal[4]);
  ESP32_ISR_Servos.setPosition(servoIndex[5], 90 + servoCal[5]);
  ESP32_ISR_Servos.setPosition(servoIndex[6], 90 + servoCal[6]);
  ESP32_ISR_Servos.setPosition(servoIndex[7], 90 + servoCal[7]);



}
String readString;

uint8_t eye_s = 0 , mouth_s=0;;
int eye_i = 0,runTime = 0,mouth_i = 149;

void loop() {

  if (deviceConnected) {
    //        pTxCharacteristic->setValue(&txValue, 1);
    //        pTxCharacteristic->notify();
    //        txValue++;
    //    delay(10); // bluetooth stack will go into congestion, if too many packets are sent
  }
//  while (Serial.available() > 0) {
//    if (deviceConnected) {
//      delay(3);
//      readString += Serial.read();
//      pTxCharacteristic->setValue(chipId.c_str());
////      pTxCharacteristic->setValue((uint32_t)ESP.getEfuseMac());
//      pTxCharacteristic->notify();
//      Serial.println(chipId);
//
//    }
//  }
  // disconnecting
  if (!deviceConnected && oldDeviceConnected) {
    delay(500); // give the bluetooth stack the chance to get things ready
    pServer->startAdvertising(); // restart advertising
    Serial.println("start advertising");
    oldDeviceConnected = deviceConnected;
  }
  // connecting
  if (deviceConnected && !oldDeviceConnected) {
    // do stuff here on connecting
    oldDeviceConnected = deviceConnected;
  }
  

   switch (cmd) {
    case 0:
      runServoPrgV(servoPrg16, servoPrg16step); // up
      cmd = 15;
      break;
    case 1:
      runServoPrgV(servoPrg17, servoPrg17step); // Get down
      cmd = 15;
      break;
    case 2:
      runServoPrgV(servoPrg10, servoPrg10step); // fighting
      break;
    case 3:
      runServoPrgV(servoPrg18, servoPrg18step); // sayH
      cmd = 0;
      break;
    case 4:
      runServoPrgV(servoPrg12, servoPrg12step); // sleep
      cmd = 15;
      break;
    case 5:
      runServoPrgV(servoPrg19, servoPrg19step); // pushUp
      cmd = 15;
      break;
    case 6:
      runServoPrgV(servoPrg20, servoPrg20step); // 
      break;
    case 7:
      runServoPrgV(servoPrg13, servoPrg13step); // dancing1
      break;
    case 8:
      runServoPrgV(servoPrg14, servoPrg14step); // dancing2
      break;

//    case 8:
//      runServoPrgV(servoPrg00, servoPrg00step); // zero
//      break;
    case 9:
      runServoPrgV(servoPrg06, servoPrg06step); // turnLeft
      break;
    case 10:
      runServoPrgV(servoPrg02, servoPrg02step); // forward
      break;
    case 11:
      runServoPrgV(servoPrg07, servoPrg07step); // turnRight
       break;
    case 12:
     runServoPrgV(servoPrg04, servoPrg04step); // moveLeft
      break;
    case 13:
      runServoPrgV(servoPrg03, servoPrg03step); // backward
       break;
    case 14:
     runServoPrgV(servoPrg05, servoPrg05step); // moveRight
       break;

       
  }

  
  if(micros()-runTime >5000){
    switch(eye_s){
      case 0:
        draweye(5,30,eye_i,10,10);
        draweye(69,30,eye_i,10,10);
        
        eye_i--;
        if(eye_i<-180){
          eye_i = -10;
          eye_s++;
        }
      break;
      case 1:
       
        draweye(5,30,0,eye_i,10);
        draweye(69,30,0,eye_i,10);
        eye_i++;
        if(eye_i >10 ){
          eye_i = 10;
          eye_s++;
        }
      break;
      case 2:
       
        draweye(5,30,0,eye_i,10);
        draweye(69,30,0,eye_i,10);
        eye_i--;
        if(eye_i <-10 ){
          eye_i = -10;
          eye_s++;
        }
      break;
      case 3:
       
        draweye(5,30,0,eye_i,10);
        draweye(69,30,0,eye_i,10);
        eye_i++;
        if(eye_i >10 ){
          eye_i = 0;
          eye_s = 0;
        }
      break;
    }

    if(mouth_s == 0){
      drawmouth(34,mouth_i);
      mouth_i++;
      if(mouth_i >159)mouth_s = 1;
    }else if(mouth_s ==1){
      drawmouth(34,mouth_i);
      mouth_i--;
      if(mouth_i < 149)mouth_s = 0;
    }
  runTime = micros();
  }
}



void runServoPrgV(int servoPrg[][numberOfACE], int step) {
  for (int i = 0; i < step; i++) { // Loop for step

    int totalTime = servoPrg[i][numberOfACE - 1]; // Total time of this step

    // Get servo start position
    for (int s = 0; s < numberOfServos; s++) {
      servoPos[s] = ESP32_ISR_Servos.getPosition(servoIndex[s]) - servoCal[s];
    }

    for (int p = 0; p < numberOfServos; p++) { // Loop for servo
      if (i == 0) {
        servoPrevPrg[p] = servoPrg[i][p];
      } else {
        servoPrevPrg[p] = servoPrevPrg[p] + servoPrg[i][p];
      }
    }

    for (int j = 0; j < totalTime / servoPrgPeriod; j++) { // Loop for time section
      for (int k = 0; k < numberOfServos; k++) { // Loop for servo
        int r = map(j, 0, totalTime / servoPrgPeriod, servoPos[k], servoPrevPrg[k]) + servoCal[k];
        ESP32_ISR_Servos.setPosition(servoIndex[k], r );
      }
      delay(servoPrgPeriod);
      
    }
  }
}

void tft_num(uint8_t x,uint8_t y,int n){
  tft.setCursor(x, y);
  tft.setTextColor(TFT_YELLOW,TFT_BLACK);
  tft.setTextFont(2);
  tft.setTextSize(1);
  if(n<10000)tft.print(0);
  if(n<1000)tft.print(0);
  if(n<100)tft.print(0);
  if(n<10)tft.print(0);
  tft.println(n);
  
}

void draweye(uint8_t x,uint8_t y,int i,int distance,uint8_t r)
{
  float sx = cos(i* DEG2RAD);
  float sy = sin(i* DEG2RAD);
  int16_t x_ = sx*distance+30;
  int16_t y_ = sy*distance+30;
  
  img.setColorDepth(8);
  img.createSprite(61,61);
  img.fillSprite(TFT_BLACK);
  
  img.fillCircle(30,30,30,TFT_BLUE);
  img.fillCircle(30,30,29,TFT_BLACK);
  img.fillCircle(x_,y_,r,TFT_WHITE);

  img.pushSprite(x,y);
  img.deleteSprite();
}

void drawmouth(uint8_t x,uint8_t y)
{
  
  img.setColorDepth(8);
  img.createSprite(67,29);
  img.fillSprite(TFT_BLACK);
  
  img.drawRoundRect(0,1,66,27,4,TFT_RED);
  img.drawRoundRect(1,2,64,25,4,TFT_RED);
  
  img.fillRoundRect(2,3,11,23,4,TFT_WHITE);
  img.fillRoundRect(14,3,11,23,4,TFT_WHITE);
  img.fillRoundRect(27,3,11,23,4,TFT_WHITE);
  img.fillRoundRect(40,3,11,23,4,TFT_WHITE);
  img.fillRoundRect(52,3,11,23,4,TFT_WHITE);
  img.drawFastHLine(2,14,62,TFT_BLACK);
  img.drawFastHLine(2,15,62,TFT_BLACK);
  
  img.pushSprite(x,y);
  img.deleteSprite();
}
